import lejos.nxt.Button;
import lejos.nxt.I2CPort;
import lejos.nxt.LCD;
import lejos.nxt.LightSensor;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.addon.CompassSensor;
import lejos.nxt.addon.IRSeeker;
import lejos.robotics.RegulatedMotor;
import lejos.util.PilotProps;

public class HelloWorld {
	public static void main(String[] args) {
		PilotProps p = new PilotProps();
		p.setProperty(PilotProps.KEY_LEFTMOTOR, "A");
		p.setProperty(PilotProps.KEY_RIGHTMOTOR, "B");
		//p.setProperty(PilotProps.KEY_REVERSE, "true");
		
		while (!Button.ESCAPE.isPressed()) {
			Motor.A.forward();
			Motor.B.forward();
			displaySensorData();
			sleep(1000);
			
			Motor.C.backward();
			displaySensorData();
			sleep(1000);
		}
	}
	
	static LightSensor light = new LightSensor(SensorPort.S1);
	static IRSeeker ir = new IRSeeker(SensorPort.S2);
	static CompassSensor compass = new CompassSensor(SensorPort.S3);
	static UltrasonicSensor ultra = new UltrasonicSensor(SensorPort.S4);
	
	public static void displaySensorData()
	{
		LCD.clear();
		LCD.drawString("Light:", 0,0);
		LCD.drawInt(light.getLightValue(), 0, 1);
		
		LCD.drawString("IR:", 1,0);
		LCD.drawInt((int)ir.getDirection(), 1,1);
		
		LCD.drawString("Compass:", 2,0);
		LCD.drawInt((int)compass.getDegrees(), 2,1);
		
		LCD.drawString("Ultra", 3,0);
		LCD.drawInt((int)ultra.getDistance(), 3,1);
	}
	
	public static void sleep(int time)
	{
		try {
			Thread.sleep(time);
		} catch (InterruptedException e) {
			e.printStackTrace();
		}
	}
}